package optimization;

import Jama.Matrix;


public class LinearAlgebra {

	public static double[] vMul(double s, double[] a)
	{
		double[] r = new double[a.length];
		for (int i = 0; i < a.length; i++) r[i] = a[i] * s;
		return r;
	}

	public static double[] vOuterMul(double[] a, double[] b)
	{
		int Na = a.length;
		int Nb = b.length;
		double[] r = new double[Na*Nb];
		for (int i = 0; i < Na; i++) {
			for (int j = 0; j < Nb; j++) {
				r[j+i*Nb] = a[i] * b[j];
			}
		}
		return r;
	}

	/**
	 * Matrix x Matrix
	 * @param a matrix
	 * @param b matrix
	 * @return a*b
	 */
	public static double[] mMul(double[] a, double[] b)
	{
		int n = (int) Math.sqrt(a.length);
		double[] mat = new double[n*n];
		
		for (int r = 0; r < n; r++) {
			for (int c = 0; c < n; c++) {
				mat[r*n+c] = 0;
				for (int k = 0; k < n; k++) {
					mat[r*n+c] += a[r*n+k] * b[k*n+c];
				}
			}
		}
		return mat;
	}

	/**
	 * Matrix inverse
	 * @param a
	 * @return
	 */
	public static double[] mInv(double[] a)
	{
		int n = (int) Math.sqrt(a.length);
		Matrix mA = new Matrix(a, n);
		Matrix mInvA = mA.inverse();
		double[] mat = new double[n*n];
		for (int r = 0; r < n; r++) {
			for (int c = 0; c < n; c++) {
				mat[r*n+c] = mInvA.get(c, r);
			}
		}
		return mat;
	}

	/**
	 * Matrix transpose
	 * @param a matrix
	 * @return transpose
	 */
	public static double[] mTrans(double[] a)
	{
		int n = (int) Math.sqrt(a.length);
		double[] mat = new double[n*n];
		
		for (int r = 0; r < n; r++) {
			for (int c = 0; c < n; c++) {
				mat[r*n+c] = a[c*n+r];
			}
		}
		return mat;
	}

	/**
	 * Matrix x Vector
	 * @param a matrix
	 * @param b vector
	 * @return vector
	 */
	public static double[] mvMul(double[] a, double[] b)
	{
		int n = (int) Math.sqrt(a.length);
		double[] v = new double[n];
		
		for (int r = 0; r < n; r++) {
			v[r] = 0;
			for (int c = 0; c < n; c++) {
				v[r] += a[r*n+c] * b[c];
			}
		}
		return v;
	}

	public static String vPrint(double[] a) 
	{
		return vPrint(a, true);
	}

	public static String vPrint(double[] a, boolean delims) 
	{
		StringBuffer sb = new StringBuffer();
		if (delims) sb.append("[");
		for (double d : a) sb.append(String.format("%.4f ", d));
		if (delims) sb.append("]");
		return sb.toString();
	}

	public static String vPrintL(double[] a) 
	{
		StringBuffer sb = new StringBuffer();
		sb.append("[");
		for (double d : a) sb.append(String.format("%.10f ", d));
		sb.append("]");
		return sb.toString();
	}
	
	public static double vMul(double[] a, double[] b)
	{
		double r = 0;
		for (int i = 0; i < a.length; i++) r += a[i] * b[i];
		return r;
	}
	
	public static double[] vAdd(double[] a, double[] b)
	{
		double[] r = new double[a.length];
		for (int i = 0; i < a.length; i++) r[i] = a[i] + b[i];
		return r;
	}

	public static double[] vSub(double[] a, double[] b)
	{
		double[] r = new double[a.length];
		for (int i = 0; i < a.length; i++) r[i] = a[i] - b[i];
		return r;
	}
	
	public static double[] vRecip(double[] a)
	{
		double[] r = new double[a.length];
		for (int i = 0; i < a.length; i++) r[i] = 1/a[i];
		return r;
	}
	
	public static double[] vAbs(double[] a)
	{
		double[] r = new double[a.length];
		for (int i = 0; i < a.length; i++) r[i] = Math.abs(a[i]);
		return r;
	}
	
	public static double vNorm2(double[] g)
	{
		double s = 0;
		for (double d : g) s += d*d;
		return Math.sqrt(s);
	}

	public static double[] vNormalize(double[] g)
	{
		return vMul(1.0/vNorm2(g), g);
	}
	
	public static double[] vRepeat(int n, double v)
	{
		double[] r = new double[n];
		for (int i = 0; i < n; i++) r[i] = v;
		return r;
	}

	public static double[] vUnit(int n, int k)
	{
		double[] r = new double[n];
		for (int i = 0; i < n; i++) r[i] = (i == k ? 1.0 : 0.0);
		return r;
	}
	
	public static double[] vSelect(double[] a, int k)
	{
		double[] r = new double[a.length];
		for (int i = 0; i < a.length; i++) r[i] = (i==k ? a[i] : 0.0);
		return r;
	}
	
	public static interface Function
	{
		public double evaluate(double[] p); 
	}

	public static interface StochasticFunction
	{
		public double evaluate(double[] p, int N, long randSeed); 
	}

	public static interface VectorFunction
	{
		public double[] evaluate(double[] p);
		public int yDim();
	}

	/**
	 * Euclidean distance between fixed point p0 and function parameter
	 * @author risto
	 *
	 */
	public static class PointDistanceFunction implements Function {
		public double[] p0;
		public PointDistanceFunction(double[] p0) {
			this.p0 = p0;
		}
		@Override
		public double evaluate(double[] p) {
			return vNorm2(vSub(p, p0));
		}
	}
	
	/**
	 * Approximate Gradient of function F by forward differences. 
	 * 
	 * J(x) = [ D f / D x1 (x),  D f / D x2 (x) ]..
	 * 
	 * To approximate the derivates, a forward difference is used:
	 * D f / D x1 = [F(x+h)-F(x)] / h
	 * 
	 * @param f function
	 * @param m number of parameters (x)
	 * @param x point where function is evaluated
	 * @param h forward difference value (e.g. 0.00001)
	 * @return gradient vector
	 */
	public static double[] gradientForwardDifferences(Function f, double[] x, double[] h)
	{
		double[] grad = new double[x.length];
		double fx = f.evaluate(x); // F(x)
		
		// for each xi
		for (int i = 0; i < x.length; i++) {
			double[] xd = x.clone(); // Copy X
			xd[i] += h[i]; // add forward difference to i:th component
			double fxi = f.evaluate(xd); // compute F at xi 
			
			double diff = (fxi - fx) / h[i]; // [F(x+h) - F(x)] / h 
			grad[i] = diff;
		}
		return grad;
	}
	
	public static double[] jacobianForwardDifferences(VectorFunction f, double[] x, double[] h)
	{
		return jacobianForwardDifferences(f, f.evaluate(x), x, h);
	}
	
	public static double[] jacobianForwardDifferences(VectorFunction f, double[] fx, double[] x, double[] h)
	{
		int n = f.yDim();
		double[] jacob = new double[n * x.length];
		
		// for each xi
		for (int i = 0; i < x.length; i++) {
			double[] xd = x.clone(); // Copy X
			xd[i] += h[i]; // add forward difference to i:th component
			double[] fxi = f.evaluate(xd); // compute F at xi 
			
			double[] diff = vMul(1/h[i], vSub(fxi, fx)); // [F(x+h) - F(x)] / h

			for (int r = 0; r < n; r++) { 
				jacob[i+r*x.length] = diff[r]; 
			}
		}
		return jacob;
	}
	
}
